#include "modules/MonitorModule/MonitorModule.h"
#include "modules/MonitorModule/VacantMonitor.h"
#include "modules/MonitorModule/TopCapObsMonitor.h"
#include "modules/MonitorModule/SawtoothWallMonitor.h"
#include "modules/MonitorModule/TiltMonitor.h"
#include "modules/MonitorModule/SlipMonitor.h"
#include "modules/MonitorModule/SmallObsMonitor.h"
#include "modules/MonitorModule/PostionStuckMonitor.h"
#include "modules/MonitorModule/BrushWindedMonitor.h"
#include "modules/MonitorModule/FanBlockMonitor.h"
#include "modules/MonitorModule/FrontPushableObsMonitor.h"

namespace behavior_controller {

MonitorModule::MonitorModule(const rclcpp::Node::SharedPtr &ros_node) : ros_node_(ros_node) {};

void MonitorModule::UpdateTime2Hz(const evt::Timer2Hz &evt) {
    //LogPub::Info("behavior_controller", "[MonitorModule] UpdateTime2Hz");
}

void MonitorModule::UpdateTime100Hz(const evt::Timer100Hz &evt) {
    LogPub::Info("behavior_controller", "[MonitorModule] UpdateTime100Hz");
    std::cout << "behavior_controller, [MonitorModule] UpdateTime100Hz" << std::endl;
    //所有的检测器都将写在这里，进行轮循检查
    //-------------------------  悬空检测开启  -------------------------//
    vacant_monitor.run();
    //------------------------- 上盖障碍检测开启 -------------------------//
    top_cap_obs_monitor.run();
    //-------------------------阶跃的墙面检测开启-------------------------//
    sawtooth_wall_monitor.run();
    //------------------------- 倾角检测开启 -------------------------//
    tilt_monitor.run();
    //------------------------- 打滑检测开启 -------------------------//
    slip_monitor.run();
    //-------------------------小障碍物检测开启-------------------------//
    small_obs_monitor.run();
    //-------------------------位置卡死检测开启-------------------------//
    postion_stuck_monitor.run();
    //-------------------------边刷缠绕检测开启-------------------------//
    brush_winded_monitor.run();
    //-------------------------风机堵转检测开启-------------------------//
    fan_block_monitor.run();
    //-------------------------可推动障碍物检测开启-------------------------//
    front_pushable_obs_monitor.run();
}

void MonitorModule::InstallFunctors() {
//    this->InstallFunction<evt::Timer2Hz>(&MonitorModule::UpdateTime2Hz);
    this->InstallFunction<evt::Timer100Hz>(&MonitorModule::UpdateTime100Hz);
}

} // namespace behavior_controller